/*
This software is distributed under the Simplified BSD License:

Copyright (c) 2008, Chris Venter <chris.venter@gmail.com>
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, 
are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice, 
    	this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice, 
    	this list of conditions and the following disclaimer in the documentation 
    	and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 
TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 
DAMAGE.
*/
#ifndef GENERIK_MATH_H
#define GENERIK_MATH_H

/** \file math.h
This is a quick and dirty file for some common linear algebra routines.
It will most probably be replaced by a wrapper for Armadillo 
(http://arma.sourceforge.net) at a future point, but for now I needed
something quick to do vector math.
*/

#include <math.h>

/*
what needs to migrate here?
point (point needs to replace translation in a number of places)
*/

namespace generik
{
	inline unsigned int min(unsigned int a, unsigned int b) { return a < b ? a : b; }
	inline unsigned int max(unsigned int a, unsigned int b) { return a > b ? a : b; }
	
	/**
	Basic draft for a point
	*/
	template <unsigned int N>
	struct point
	{
		double data[N];
		double& operator[](unsigned int n) { return data[n]; }
		double operator[](unsigned int n) const { return data[n]; }
		
		point(double* d)
		{
			for (int n = N-1; n != 0; --n) data[n] = d[n];
		}
		
		template <unsigned int M>
		point& operator+=(const point<M>& p)
		{
			for (int n = min(M, N)-1; n != 0; --n)
			{
				data[n]+=p.data[n];
			}
			return *this;
		}
		template <unsigned int M>
		point<(M < N ? M : N)>& operator+(const point<M>& p)
		{
			point<(M < N ? M : N)> result;
			for (int n = min(M, N)-1; n != 0; --n)
			{
				result.data[n] = data[n] + p.data[n];
			}
			return result;
		}
		
	};
	
	template <>
	struct point<4>
	{
		union
		{
			double data[4];
			struct
			{
				double x, y, z, w;
			};
		};
		point(double x_, double y_, double z_, double w_)
		: x(x_), y(y_), z(z_), w(w_)
		{}
		point& operator+=(const point& p)
		{
			x+=p.x; y+=p.y; z+=p.z; w+=p.w;
			return *this;
		}
		
	};
	
	/*
	struct point4
	{
		double x, y, z, w;
		point4()
		{}
		point4(double x_, double y_, double z_, double w_)
		: x(x_), y(y_), z(z_), w(w_)
		{}
		void set(double x_, double y_, double z_, double w_) { x = x_; y = y_; z = z_; w = w_; }
		point4 operator+(const point4& p)
		{
			return point4(x+p.x, y+p.y, z+p.z, w+p.w);
		}
		point4& operator+=(const point4& p)
		{
			x+=p.x; y+=p.y; z+=p.z; w+=p.w;
			return *this;
		}
	};
	struct point3
	{
		double x, y, z;
		point3()
		{}
		point3(double x_, double y_, double z_)
		: x(x_), y(y_), z(z_)
		{}
		void set(double x_, double y_, double z_) { x = x_; y = y_; z = z_; }
	};
	struct point2
	{
		double x, y;
		point2()
		{}
		point2(double x_, double y_)
		: x(x_), y(y_)
		{}
		void set(double x_, double y_) { x = x_; y = y_; }
	};
	*/
	
	struct translation
	{
		double x, y, z, w;
		
		/*
		operator point4 () { return point4(x, y, z, 1.0); }
		operator point3&() { return (point3&)(*this); }
		operator point2&() { return (point2&)(*this); }
		*/
		
		void set(double xin, double yin, double zin, double win = 1.0)
		{
			x = xin;
			y = yin;
			z = zin;
			w = win;
		}
	
		translation()
		{}
		
		translation(double xin, double yin, double zin, double win = 1.0)
		{
			set(xin, yin, zin, win);
		}
		
		/** Invert the translation.
		Same as a 180 degree rotation, w is unchanged.
		*/
		translation inv()
		{
			translation t;
			t.set(-x, -y, -z, w);
			return t;
		}
		
		translation operator+(const translation& t)
		{
			translation r(x + t.x, y + t.y, z + t.z, w);
			return r;
		}
		translation& operator+=(const translation& t)
		{
			set(x + t.x, y + t.y, z + t.z, w);
			return *this;
		}
		translation operator-(const translation& t)
		{
			translation r(x - t.x, y - t.y, z - t.z, w);
			return r;
		}
		translation& operator-=(const translation& t)
		{
			set(x - t.x, y - t.y, z - t.z, w);
			return *this;
		}
		
		translation operator*(double scalar) const
		{
			translation t(scalar*x, scalar*y, scalar*z, w);
			return t;
		}
		translation& operator*=(double scalar)
		{
			set(scalar*x, scalar*y, scalar*z, w);
			return *this;
		}
		
		double magnitude() const
		{
			return sqrt(magnitude2());
		}
		double magnitude2() const
		{
			return x*x + y*y + z*z;
		}
	
		void normalise()
		{
			double m = magnitude();
			if (m > 0.0)
			{
				x /= m;
				y /= m;
				z /= m;
				w = 1.0;
			}
		}
	};

	translation operator*(double scalar, const translation& tin)
	{
		translation t = tin*scalar;
		return t;
	}
	
	struct axisangle
	{
		double angle;
		double x, y, z;
	
		void set()
		{
			set(0.0, 1.0, 0.0, 0.0);
		}
		void set(double ain, double xin, double yin, double zin)
		{
			angle = ain;
			x = xin;
			y = yin;
			z = zin;
		}
	
		axisangle()
		{}
		axisangle(double a, const translation& t)
		{
			set(a, t.x, t.y, t.z);
		}
		axisangle(double a, double x, double y, double z)
		{
			set(a, x, y, z);
		}
	
		axisangle inv()
		{
			axisangle a;
			a.set(-angle, x, y, z);
			return a;
		}
	
		axisangle& operator*=(const axisangle& a);
	};

	struct color
	{
		double r, g, b, a;
		
		color()
		{}
		
		color(double rin, double gin, double bin, double ain = 1.0)
		: r(rin), g(gin), b(bin), a(ain)
		{}
		
		void set()
		{
			set(1.0, 1.0, 1.0);
		}
		void set(double rin, double gin, double bin, double ain = 1.0)
		{
			r = rin;
			g = gin;
			b = bin;
			a = ain;
		}
	};

	struct bbox;

	struct sphere
	{
		double radius;
		double x, y, z;
		
		sphere()
		{}
		sphere(double r)
		: radius(r)
		, x(0), y(0), z(0)
		{}
		sphere(double rin, double xin, double yin, double zin)
		{
			set(rin, xin, yin, zin);
		}
		void set(double rin, double xin, double yin, double zin)
		{
			radius = rin;
			x = xin;
			y = yin;
			z = zin;
		}
	
		void set(double rin, const translation& t)
		{
			radius = rin;
			x = t.x;
			y = t.y;
			z = t.z;
		}
	
		sphere(const bbox& b);
		
		sphere& operator+=(const translation& t)
		{
			set(radius, x + t.x, y +t.y, z + t.z);
			return *this;
		}
		
		sphere operator+(const translation& t) const
		{
			sphere s(radius, x + t.x, y +t.y, z + t.z);
			return s;
		}
			

	};

	struct bbox
	{
		double x0, x1;
		double y0, y1;
		double z0, z1;
	
		bbox()
		{}
	
		bbox(double x0_, double x1_, double y0_, double y1_, double z0_, double z1_)
		{
			set(x0_, x1_, y0_, y1_, z0_, z1_);
		}
	
		bbox(double r, const translation& t)

		{
			set(t.x-r, t.x+r, t.y-r, t.y+r, t.z-r, t.z+r);
		}
	
		bbox(double r)
		{
			set(r);
		}
	
		bbox(const sphere& s);
	
		void set(double r)
		{
			x0 = y0 = z0 = -r;
			x1 = y1 = z1 = +r;
		}
	
		void set(double x0_, double x1_, double y0_, double y1_, double z0_, double z1_)
		{
			x0 = x0_;
			x1 = x1_;
			y0 = y0_;
			y1 = y1_;
			z0 = z0_;
			z1 = z1_;
		}
	
		void swallow(const bbox& bb)
		{
			if (bb.x0 < x0) x0 = bb.x0;
			if (bb.x1 > x1) x1 = bb.x1;
			if (bb.y0 < y0) y0 = bb.y0;
			if (bb.y1 > y1) y1 = bb.y1;
			if (bb.z0 < z0) z0 = bb.z0;
			if (bb.z1 > z1) z1 = bb.z1;
		}
	
		double width() const
		{
			return x1-x0;
		}
		double height() const
		{
			return y1-y0;
		}
		double depth() const
		{
			return z1-z0;
		}
		
		double diagonal2() const
		{
			return width()*width() + height()*height() + depth()*depth();
		}
		double diagonal() const
		{
			return sqrt(diagonal2());
		}
		double radius() const
		{
			return diagonal()/2.0; //sqrt(width()*width() + height()*height() + depth()*depth())/2.0;
		}
		double volume() const
		{
			return width()*height()*depth();
		}
		double area() const
		{
			return 2*width()*height() + 2*width()*depth() + 2*height()*depth();
		}
	
		const translation center() const
		{
			translation t;
			t.set((x1+x0)/2.0, (y1+y0)/2.0, (z1+z0)/2.0);
			return t;
		}
	
		/*
		sphere get_sphere()
		{
			sphere s;
			s.set(radius(), center());
		}
		*/
		
		bool overlaps_x(const bbox& box)
		{
			return (x0 <= box.x1 && x1 >= box.x0);
		}
		bool overlaps_y(const bbox& box)
		{
			return (y0 <= box.y1 && y1 >= box.y0);
		}
		bool overlaps_z(const bbox& box)
		{
			return (z0 <= box.z1 && z1 >= box.z0);
		}
		
		bool overlaps(const bbox& box)
		{
			return overlaps_x(box) && overlaps_y(box) && overlaps_z(box);
		}
		
		bbox(const std::list<bbox*>& boxes)
		{
			std::list<bbox*>::const_iterator i = boxes.begin();
			if (i != boxes.end())
			{
				*this = **i;
				while (++i != boxes.end())
				{
					bbox* b = *i;
					if (b->x0 < x0) x0 = b->x0;
					if (b->x1 > x1) x1 = b->x1;
					if (b->y0 < y0) y0 = b->y0;
					if (b->y1 > y1) y1 = b->y1;
					if (b->z0 < z0) z0 = b->z0;
					if (b->z0 > z0) z1 = b->z1;
				}
			}
		}
		
		/* // needs point before we can do this
		
		void setpoint(point& p)
		{
			x0 = x1 = p.x;
			y0 = y1 = p.y;
			z0 = z1 = p.z;
		}
		void addpoint(point& p)
		{
			if (p.x < x0) x0 = p.x;
			if (p.x > x1) x1 = p.x;
			if (p.y < y0) y0 = p.y;
			if (p.y > y1) y1 = p.y;
			if (p.z < z0) z0 = p.z;
			if (p.z > z1) z1 = p.z;
		}
	
		bbox(const std::list<point*>& points)
		{
			std::list<point*>::const_iterator i = points.begin();
			if (i != points.end())
			{
				setpoint(*i);
				while (++i != points.end())
				{
					addpoint(*i);
				}
			}
		}
		*/
		
		bbox operator+(const translation& t) const
		{
			bbox b(
				x0 + t.x, x1 + t.x,
				y0 + t.y, y1 + t.y,
				z0 + t.z, z1 + t.z
			);
			return b;
		}
		bbox& operator+=(const translation& t)
		{
			set(
				x0 + t.x, x1 + t.x,
				y0 + t.y, y1 + t.y,
				z0 + t.z, z1 + t.z
			);
			return *this;
		}
	};
	
	bbox operator+(const translation& t, const bbox& b)
	{
		return b.operator+(t);
	}
	
	/*
	// needs point before we can do this:
	
	bool point_in_box_x(point* p, bbox* box)
	{
		return (p->x >= box->x0 && p->x <= box->x1); 
	}
	bool point_in_box_y(point* p, bbox* box)
	{
		return (p->y >= box->y0 && p->y <= box->y1); 
	}
	bool point_in_box_z(point* p, bbox* box)
	{
		return (p->z >= box->z0 && p->z <= box->z1); 
	}

	bool point_in_box(point* p, bbox* box)
	{
		return point_in_box_x(p, box) && point_in_box_y(p, box) && point_in_box_z(p, box);
	}
	*/

	sphere::sphere(const bbox& b)
	{
		radius = b.radius()/sqrt(2.0);
		translation t = b.center();
		x = t.x;
		y = t.y;
		z = t.z;
	}

	/**
	most of the math on this comes from
	http://www.j3d.org/matrix_faq/matrfaq_latest.html
	*/
	struct quaternion
	{
		double w, x, y, z;
	
		quaternion()
		{}
	
		quaternion(const axisangle& a)
		{
			double sin_a = sin(M_PI*a.angle/(2.0*180.0));
			double cos_a = cos(M_PI*a.angle/(2.0*180.0));
			w = cos_a;
			x = a.x*sin_a;
			y = a.y*sin_a;
			z = a.z*sin_a;
		}
	
		operator axisangle()
		{
			axisangle a;
			double cos_a = w;
			a.angle = 2*180.0 * acos(cos_a) / M_PI;
			double sin_a = sqrt( 1.0 - cos_a * cos_a );
			if (fabs(sin_a) < 0.0005) sin_a = 1.0;
			a.x = x / sin_a;
			a.y = y / sin_a;
			a.z = z / sin_a;
			return a;
		}
		
		void set(double w_, double x_, double y_, double z_)
		{
			w = w_;
			x = x_;
			y = y_;
			z = z_;
		}
		
		quaternion& normalise()
		{
			double E = w*w + x*x + y*y + z*z;
			if (E > 0.0)
			{
				double sE = sqrt(E);
				w /= sE;
				x /= sE;
				y /= sE;
				z /= sE;
			}
			return *this;
		}
	};

	quaternion operator*(const quaternion& q1, const quaternion& q2)
	{
		quaternion r;
		r.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
		r.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y;
		r.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z;
		r.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x;
		return r;
	}

	axisangle operator*(const axisangle& a1, const axisangle& a2)
	{
		//quaternion q1(a1);
			double sin_a1 = sin(M_PI*a1.angle/(2.0*180.0));
			double cos_a1 = cos(M_PI*a1.angle/(2.0*180.0));
		//quaternion q2(a2);
			double sin_a2 = sin(M_PI*a2.angle/(2.0*180.0));
			double cos_a2 = cos(M_PI*a2.angle/(2.0*180.0));
		//quaternion r = q1*q2;
		double sin_aa = sin_a1*sin_a2;
		double cos_sin = cos_a1*sin_a2;
		double sin_cos = sin_a1*cos_a2;
		quaternion r;
		r.w = cos_a1*cos_a2 - sin_aa*(a1.x*a2.x + a1.y*a2.y + a1.z*a2.z);
		// r.w = cos_a1*cos_a2 - sin_a1*sin_a2*dot(a1, a2);
		// ie. r.w^2 = cos_a1*cos_a2*cos_a1*cos_a2 - 2*cos_a1*cos_a2*sin_a1*sin_a2*dot(a1, a2) + sin_a1*sin_a2*dot(a1, a2)*sin_a1*sin_a2*dot(a1, a2);
		// = cos2_a1*cos2_a2 - 2*cos_a1*cos_a2*sin_a1*sin_a2*dot(a1, a2) + sin2_a1*sin2_a2*dot2(a1, a2);
		r.x = cos_sin*a2.x + sin_cos*a1.x + sin_aa*(a1.y*a2.z - a1.z*a2.y);
		r.y = cos_sin*a2.y + sin_cos*a1.y + sin_aa*(a1.z*a2.x - a1.x*a2.z);
		r.z = cos_sin*a2.z + sin_cos*a1.z + sin_aa*(a1.x*a2.y - a1.y*a2.x);
		//axisangle a = r;
		axisangle a;
		double cos_a = r.w;
		a.angle = 2*180.0 * acos(r.w) / M_PI;
		double sin_a = sqrt( 1.0 - r.w * r.w );
		if (fabs(sin_a) < 0.0005) sin_a = 1.0;
		a.x = r.x / sin_a;
		a.y = r.y / sin_a;
		a.z = r.z / sin_a;
		return a;
	}

	axisangle& axisangle::operator*=(const axisangle& a)
	{
		(*this) = (*this) * a;
		return *this;
	}

	translation cross(const translation& t1, const translation& t2)
	{
		translation t;
		t.x = t1.y*t2.z - t1.z*t2.y;
		t.y = t1.z*t2.x - t1.x*t2.z;
		t.z = t1.x*t2.y - t1.y*t2.x;
		return t;
	}

	double dot(const translation& t1, const translation& t2)
	{
		return t1.x*t2.x + t1.y*t2.y + t1.z*t2.z;
	}

	double angle(const translation& t1, const translation& t2)
	{
		double m1 = t1.magnitude2();
		if (m1 > 0)
		{
			double m2 = t2.magnitude2();
			double m = m1*m2;
			if (m > 0)
			{
				return 180.0*acos(dot(t1, t2) / sqrt(m))/M_PI;
			}
		}
		return 0.0;
	}
	

	bbox::bbox(const sphere& s)
	{
		set(s.x-s.radius, s.x+s.radius, s.y-s.radius, s.y+s.radius, s.z-s.radius, s.z+s.radius);
	}
	
	struct matrix4
	{
		union
		{
			double m[4*4];
			double i[4][4];
			struct
			{
				double a11, a12, a13, a14;
				double a21, a22, a23, a24;
				double a31, a32, a33, a34;
				double a41, a42, a43, a44;
			};
		};
		
		matrix4()
		{}
		
		double& operator[](int x)
		{
			return m[x];
		}
		double operator[](int x) const
		{
			return m[x];
		}
		
	/*
		matrix4(double value)
		{
			a11 = a12 = a13 = a14 = value;
			a21 = a22 = a23 = a24 = value;
			a31 = a32 = a33 = a34 = value;
			a41 = a42 = a43 = a44 = value;
		}
		
		void set(const axisangle& a)
		{
			double rcos = cos(a.angle);
			double rsin = sin(a.angle);
			a11 =        rcos + a.x*a.x*(1-rcos);
			a21 =  a.z * rsin + a.y*a.x*(1-rcos);
			a31 = -a.y * rsin + a.z*a.x*(1-rcos);
			a12 = -a.z * rsin + a.x*a.y*(1-rcos);
			a22 =        rcos + a.y*a.y*(1-rcos);
			a32 =  a.x * rsin + a.z*a.y*(1-rcos);
			a13 =  a.y * rsin + a.x*a.z*(1-rcos);
			a23 = -a.x * rsin + a.y*a.z*(1-rcos);
			a33 =        rcos + a.z*a.z*(1-rcos);
		}
		void set(const translation& t)
		{
			a14 = t.x;
			a24 = t.y;
			a34 = t.z;
		}
		
		matrix4(const axisangle& a, const translation& t)
		{
			set(a);
			set(t);
			a41 = a42 = a43 = 0.0;
			a44 = 1.0;
		}
				
		matrix4& operator*=(const matrix4& q)
		{
			return *this = *this * m;
		}
		
		matrix4 inv()
		{
			matrix4 q;
			q
		}
		*/
		
		matrix4 transpose()
		{
			matrix4 p;
			for (int i = 0; i < 4; ++i)
			{
				for (int j = 0; j < 4; ++j)
				{
					p[i+j*4] = m[j+4*i];
				}
			}
			return p;
		}

		translation operator*(const translation& t)
		{
			translation result;
			result.x = t.x*a11 + t.y*a12 + t.z*a13 + t.w*a14;
			result.y = t.x*a21 + t.y*a22 + t.z*a23 + t.w*a24;
			result.z = t.x*a31 + t.y*a32 + t.z*a33 + t.w*a34;
			return result;
		}
		double trace() const
		{
			return 1 + m[0] + m[5] + m[10];
		}
	};
	
	/*
	matrix4 eye()
	{
		matrix4 m;
		m.a12 = m.a13 = m.a14 = m.a21 = m.a23 = m.a24 = m.a31 = m.a32 = m.a34 = m.a41 = m.a42 = m.a43 = 0.0;
		m.a11 = m.a22 = m.a33 = m.a44 = 1.0;
		return m;
	}
	*/
	matrix4 matrix4gl(double* g)
	{
		matrix4 p;
		for (int i = 0; i < 4; ++i)
		{
			for (int j = 0; j < 4; ++j)
			{
				p[i+j*4] = g[j+4*i];
			}
		}
		return p;
	}
	
	void decompose(const generik::matrix4& m4, generik::translation& t)
	{
		// translation is easy:
		t.set(m4.a14, m4.a24, m4.a34);
	}
	void decompose(const generik::matrix4& m4, quaternion& q)
	{
		// rotation is much harder:
		double S, X, Y, Z, W;
		double trace = m4.trace();
		if (trace > 0.00000001)
		{
			S = sqrt(trace) * 2;
			X = (m4.m[9] - m4.m[6]) / S;
			Y = (m4.m[2] - m4.m[8]) / S;
			Z = (m4.m[4] - m4.m[1]) / S;
			W = 0.25 * S;
		}
		else
		{
			if ( m4.m[0] > m4.m[5] && m4.m[0] > m4.m[10] )
			{
				// Column 0: 
				S  = sqrt( 1.0 + m4.m[0] - m4.m[5] - m4.m[10] ) * 2;
				X = 0.25 * S;
				Y = (m4.m[4] + m4.m[1] ) / S;
				Z = (m4.m[2] + m4.m[8] ) / S;
				W = (m4.m[9] - m4.m[6] ) / S;
			}
			else if ( m4.m[5] > m4.m[10] )
			{
				// Column 1: 
				S  = sqrt( 1.0 + m4.m[5] - m4.m[0] - m4.m[10] ) * 2;
				X = (m4.m[4] + m4.m[1] ) / S;
				Y = 0.25 * S;
				Z = (m4.m[9] + m4.m[6] ) / S;
				W = (m4.m[2] - m4.m[8] ) / S;
			}
			else
			{
				// Column 2:
				S  = sqrt( 1.0 + m4.m[10] - m4.m[0] - m4.m[5] ) * 2;
				X = (m4.m[2] + m4.m[8] ) / S;
				Y = (m4.m[9] + m4.m[6] ) / S;
				Z = 0.25 * S;
				W = (m4.m[4] - m4.m[1] ) / S;
			}
		}
		q.set(W, X, Y, Z);
		q.normalise();
	}
	void decompose(const generik::matrix4& m4, axisangle& a)
	{
		quaternion q;
		decompose(m4, q);
		a = q;
	}
	void decompose(const generik::matrix4& m4, generik::translation& t, generik::axisangle& a)
	{
		decompose(m4, t);
		decompose(m4, a);
		/*
		double cos_a = W;
		double sin_a = sqrt(1.0 - cos_a*cos_a);
		if (fabs(sin_a) < 0.0005) sin_a = 1.0;
		double ax = X / sin_a;
		double ay = Y / sin_a;
		double az = Z / sin_a;
		double ang = acos(W) * 2;
		double E = sqrt(ax*ax + ay*ay + az*az + ang*ang);
		if (E > 0.0)
		{
			ax  /= E;
			ay  /= E;
			az  /= E;
			ang /= E;
		}
		a.angle = 180.0*ang/M_PI;
		a.x = ax;
		a.y = ay;
		a.z = az;
		*/
	}
		
	std::ostream& operator<<(std::ostream& stream, const translation& t)
	{
		return stream<<"("<<t.x<<", "<<t.y<<", "<<t.z<<")";
	}

	std::ostream& operator<<(std::ostream& stream, const axisangle& a)
	{
		return stream<<a.angle<<"("<<a.x<<", "<<a.y<<", "<<a.z<<")";
	}

	std::ostream& operator<<(std::ostream& stream, const bbox& b)
	{
		return stream<<"["<<b.x0<<", "<<b.x1<<"; "<<b.y0<<", "<<b.y1<<"; "<<b.z0<<", "<<b.z1<<"]";
	}

	std::ostream& operator<<(std::ostream& stream, const sphere& s)
	{
		return stream<<s.radius<<"("<<s.x<<", "<<s.y<<", "<<s.z<<")";
	}
	/*
	std::ostream& operator<<(std::ostream& stream, const matrix4& M)
	{
		stream<<M.a11<<", "<<M.a12<<", "<<M.a13<<", "<<M.a14<<"\n";
		stream<<M.a21<<", "<<M.a22<<", "<<M.a23<<", "<<M.a24<<"\n";
		stream<<M.a31<<", "<<M.a32<<", "<<M.a33<<", "<<M.a34<<"\n";
		stream<<M.a41<<", "<<M.a42<<", "<<M.a43<<", "<<M.a44;
		return stream;
	}
	*/
	std::ostream& operator<<(std::ostream& stream, const matrix4& M)
	{
		for (int j = 0; j < 4; ++j)
		{
			for (int i = 0; i < 4; ++i)
			{
				stream<<M.m[i + j*4];
				if (i < 3) stream<<"\t\t ";
			}
			if (j < 3) stream<<"\n";
		}
		return stream;
	}
}

#endif








